#include "capture-decoder.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <errno.h>
#include <vcapture/capture.h>

#include "capture-imx8x-ext.h"
#include "capture_platform.h"
#include "capture-decoder-camera.h"

#define TP2855_REG_PAGE					0x40
#define TP2855_REG_VIDEO_INPUT_STATUS	0x01
#define TP2855_REG_DEVICE_ID1			0xFE
#define TP2855_REG_DEVICE_ID2			0xFF
#define TP2855_DEVICE_ID1				0x28
#define TP2855_DEVICE_ID2				0x55

#define TP2855_PAGE_CH0					0
#define TP2855_PAGE_CH1					1
#define TP2855_PAGE_CH2					2
#define TP2855_PAGE_CH3					3
#define TP2855_PAGE_CH_ALL				4
#define TP2855_PAGE_MIPI				8

static int camera_type = 1;
static capture_log_context_t log_context;

typedef enum _dev_addr_type {
	DEV_ADDR_TYPE_DECODER,
	DEV_ADDR_TYPE_CAMERA_DEFAULT,
	DEV_ADDR_TYPE_CAMERA_USE,
	DEV_ADDR_TYPE_CAMERA_CUSTOMER_1,
} dev_addr_type_t;

typedef struct _init_data {
	dev_addr_type_t addr_type;
	uint8_t reg;
	uint8_t data;
	uint32_t delay;
} init_data_t;

typedef struct _check_data {
	dev_addr_type_t addr_type;
	uint8_t reg;
	uint8_t check;
	uint8_t mask;
} check_data_t;

static int capture_get_camera_status(capture_context_t context);
static int capture_decoder_disable(capture_context_t context);
static int capture_decoder_enable(capture_context_t context);
static int decoder_enable(capture_context_t context, int enable);
static int tp2855_open(capture_context_t context);
static void capture_dump_regs(int fd, uint32_t addr);
static void capture_dump_decoder_regs(capture_context_t context);
static int capture_decoder_camera_init(capture_context_t context);
static int capture_decoder_camera_status_check(capture_context_t context);
#if 0
static init_data_t decoder_camra_init_data[] = {
	/* TP2855_common_init begin */
	{DEV_ADDR_TYPE_DECODER, TP2855_REG_PAGE, TP2855_PAGE_MIPI, 0},
	{DEV_ADDR_TYPE_DECODER, 0x23, 0x02, 0},
	/* TP2855_decoder_init begin */
	{DEV_ADDR_TYPE_DECODER, TP2855_REG_PAGE, TP2855_PAGE_CH_ALL, 0},
	{DEV_ADDR_TYPE_DECODER, 0x45, 0x01, 0},
	{DEV_ADDR_TYPE_DECODER, 0x06, 0x12, 0},
	{DEV_ADDR_TYPE_DECODER, 0x26, 0x04, 0},
	/* FHD25 begin */
	{DEV_ADDR_TYPE_CAMERA_CUSTOMER_1, 0xF5, 0x00, 0},
	{DEV_ADDR_TYPE_DECODER, 0x02, 0x40, 0},
	{DEV_ADDR_TYPE_DECODER, 0x07, 0xC0, 0},
	{DEV_ADDR_TYPE_DECODER, 0x0B, 0xC0, 0},
	{DEV_ADDR_TYPE_DECODER, 0x0C, 0x03, 0},
	{DEV_ADDR_TYPE_DECODER, 0x0D, 0x50, 0},
	{DEV_ADDR_TYPE_DECODER, 0x15, 0x03, 0},
	{DEV_ADDR_TYPE_DECODER, 0x16, 0xD2, 0},
	{DEV_ADDR_TYPE_DECODER, 0x17, 0x80, 0},
	{DEV_ADDR_TYPE_DECODER, 0x18, 0x29, 0},
	{DEV_ADDR_TYPE_DECODER, 0x19, 0x38, 0},
	{DEV_ADDR_TYPE_DECODER, 0x1A, 0x47, 0},
	{DEV_ADDR_TYPE_DECODER, 0x1C, 0x0A, 0},
	{DEV_ADDR_TYPE_DECODER, 0x1D, 0x50, 0},
	{DEV_ADDR_TYPE_DECODER, 0x20, 0x30, 0},
	{DEV_ADDR_TYPE_DECODER, 0x21, 0x84, 0},
	{DEV_ADDR_TYPE_DECODER, 0x22, 0x36, 0},
	{DEV_ADDR_TYPE_DECODER, 0x23, 0x3C, 0},
	{DEV_ADDR_TYPE_DECODER, 0x2B, 0x60, 0},
	{DEV_ADDR_TYPE_DECODER, 0x2C, 0x0A, 0},
	{DEV_ADDR_TYPE_DECODER, 0x2D, 0x30, 0},
	{DEV_ADDR_TYPE_DECODER, 0x2E, 0x70, 0},
	{DEV_ADDR_TYPE_DECODER, 0x30, 0x48, 0},
	{DEV_ADDR_TYPE_DECODER, 0x31, 0xBB, 0},
	{DEV_ADDR_TYPE_DECODER, 0x32, 0x2E, 0},
	{DEV_ADDR_TYPE_DECODER, 0x33, 0x90, 0},
	{DEV_ADDR_TYPE_DECODER, 0x35, 0x05, 0},
	{DEV_ADDR_TYPE_DECODER, 0x38, 0x00, 0},
	{DEV_ADDR_TYPE_DECODER, 0x39, 0x1C, 0},
	/* STD_HDA begin */
	{DEV_ADDR_TYPE_DECODER, 0x02, 0x44, 0},
	{DEV_ADDR_TYPE_DECODER, 0x0D, 0x73, 0},
	{DEV_ADDR_TYPE_DECODER, 0x15, 0x01, 0},
	{DEV_ADDR_TYPE_DECODER, 0x16, 0xF0, 0},
	{DEV_ADDR_TYPE_DECODER, 0x18, 0x2A, 0},
	{DEV_ADDR_TYPE_DECODER, 0x20, 0x3C, 0},
	{DEV_ADDR_TYPE_DECODER, 0x21, 0x46, 0},
	{DEV_ADDR_TYPE_DECODER, 0x25, 0xFE, 0},
	{DEV_ADDR_TYPE_DECODER, 0x26, 0x0D, 0},
	{DEV_ADDR_TYPE_DECODER, 0x2C, 0x3A, 0},
	{DEV_ADDR_TYPE_DECODER, 0x2D, 0x54, 0},
	{DEV_ADDR_TYPE_DECODER, 0x2E, 0x40, 0},
	{DEV_ADDR_TYPE_DECODER, 0x30, 0xA5, 0},
	{DEV_ADDR_TYPE_DECODER, 0x31, 0x86, 0},
	{DEV_ADDR_TYPE_DECODER, 0x32, 0xFB, 0},
	{DEV_ADDR_TYPE_DECODER, 0x33, 0x60, 0},
	/* STD_HDA end */
	/* FHD25 end */
	/* TP2855_decoder_init end */
	/* TP2855_common_init end */

	/* TP2855_mipi_out begin */
	{DEV_ADDR_TYPE_DECODER, TP2855_REG_PAGE, TP2855_PAGE_MIPI, 0},
	{DEV_ADDR_TYPE_DECODER, 0x01, 0xF0, 0},
	{DEV_ADDR_TYPE_DECODER, 0x02, 0x01, 0},
	{DEV_ADDR_TYPE_DECODER, 0x08, 0x0F, 0},
	/* MIPI_4CH4LANE_594M begin */
	{DEV_ADDR_TYPE_DECODER, 0x20, 0x44, 0},
	{DEV_ADDR_TYPE_DECODER, 0x34, 0xE4, 0},
	{DEV_ADDR_TYPE_DECODER, 0x15, 0x0C, 0},
	{DEV_ADDR_TYPE_DECODER, 0x25, 0x08, 0},
	{DEV_ADDR_TYPE_DECODER, 0x26, 0x06, 0},
	{DEV_ADDR_TYPE_DECODER, 0x27, 0x11, 0},
	{DEV_ADDR_TYPE_DECODER, 0x29, 0x0A, 0},
	{DEV_ADDR_TYPE_DECODER, 0x33, 0x07, 0},
	{DEV_ADDR_TYPE_DECODER, 0x33, 0x00, 0},
	{DEV_ADDR_TYPE_DECODER, 0x14, 0x33, 0},
	{DEV_ADDR_TYPE_DECODER, 0x14, 0xB3, 0},
	{DEV_ADDR_TYPE_DECODER, 0x14, 0x33, 0},
	/* MIPI_4CH4LANE_594M end */
	/* TP2855_mipi_out end */
};
#else
static init_data_t decoder_camra_init_data[] = {
	/* TP2855_common_init begin */
	{DEV_ADDR_TYPE_DECODER, TP2855_REG_PAGE, TP2855_PAGE_MIPI, 0},
	{DEV_ADDR_TYPE_DECODER, 0x23, 0x02, 0},
	/* TP2855_decoder_init begin */
	{DEV_ADDR_TYPE_DECODER, TP2855_REG_PAGE, TP2855_PAGE_CH_ALL, 0},
	{DEV_ADDR_TYPE_DECODER, 0x45, 0x01, 0},
	{DEV_ADDR_TYPE_DECODER, 0x06, 0x12, 0},
	{DEV_ADDR_TYPE_DECODER, 0x26, 0x04, 0},
	/* FHD25 begin */
	{DEV_ADDR_TYPE_CAMERA_CUSTOMER_1, 0xF5, 0xFF, 0},

	{DEV_ADDR_TYPE_DECODER, 0x06, 0x32, 0},
	{DEV_ADDR_TYPE_DECODER, 0x02, 0x47, 0},
	{DEV_ADDR_TYPE_DECODER, 0x07, 0x80, 0},
	{DEV_ADDR_TYPE_DECODER, 0x0B, 0x80, 0},
	{DEV_ADDR_TYPE_DECODER, 0x0C, 0x13, 0},
	{DEV_ADDR_TYPE_DECODER, 0x0D, 0x51, 0},

	{DEV_ADDR_TYPE_DECODER, 0x15, 0x03, 0},
	{DEV_ADDR_TYPE_DECODER, 0x16, 0xF0, 0},
	{DEV_ADDR_TYPE_DECODER, 0x17, 0xA0, 0},
	{DEV_ADDR_TYPE_DECODER, 0x18, 0x17, 0},
	{DEV_ADDR_TYPE_DECODER, 0x19, 0x20, 0},
	{DEV_ADDR_TYPE_DECODER, 0x1A, 0x15, 0},
	{DEV_ADDR_TYPE_DECODER, 0x1C, 0x06, 0},
	{DEV_ADDR_TYPE_DECODER, 0x1D, 0xC0, 0},
	//1080p@25fps
//	{DEV_ADDR_TYPE_DECODER, 0x1C, 0x0A, 0},
//	{DEV_ADDR_TYPE_DECODER, 0x1D, 0x50, 0},
	//
	//720p@25fps
//	{DEV_ADDR_TYPE_DECODER, 0x1C, 0x07, 0},
//	{DEV_ADDR_TYPE_DECODER, 0x1D, 0xBC, 0},
	//
	{DEV_ADDR_TYPE_DECODER, 0x20, 0x48, 0},
	{DEV_ADDR_TYPE_DECODER, 0x21, 0x84, 0},
	{DEV_ADDR_TYPE_DECODER, 0x22, 0x37, 0},
	{DEV_ADDR_TYPE_DECODER, 0x23, 0x3F, 0},

	{DEV_ADDR_TYPE_DECODER, 0x2B, 0x70, 0},
	{DEV_ADDR_TYPE_DECODER, 0x2C, 0x2A, 0},
	{DEV_ADDR_TYPE_DECODER, 0x2D, 0x4B, 0},
	{DEV_ADDR_TYPE_DECODER, 0x2E, 0x56, 0},

	{DEV_ADDR_TYPE_DECODER, 0x30, 0x7A, 0},
	{DEV_ADDR_TYPE_DECODER, 0x31, 0x4A, 0},
	{DEV_ADDR_TYPE_DECODER, 0x32, 0x4D, 0},
	{DEV_ADDR_TYPE_DECODER, 0x33, 0xFB, 0},

	{DEV_ADDR_TYPE_DECODER, 0x35, 0x65, 0},
	{DEV_ADDR_TYPE_DECODER, 0x38, 0x00, 0},
	{DEV_ADDR_TYPE_DECODER, 0x39, 0x04, 0},
	/* STD_HDA begin */
	//1080p
//	{DEV_ADDR_TYPE_DECODER, 0x02, 0x44, 0},
//	{DEV_ADDR_TYPE_DECODER, 0x0D, 0x73, 0},
	//
	//720p
//	{DEV_ADDR_TYPE_DECODER, 0x02, 0x46, 0},
//	{DEV_ADDR_TYPE_DECODER, 0x0D, 0x71, 0},
//	//
//	{DEV_ADDR_TYPE_DECODER, 0x18, 0x1B, 0},
//	{DEV_ADDR_TYPE_DECODER, 0x20, 0x40, 0},
//	{DEV_ADDR_TYPE_DECODER, 0x21, 0x46, 0},
//
//	{DEV_ADDR_TYPE_DECODER, 0x25, 0xFE, 0},
//	{DEV_ADDR_TYPE_DECODER, 0x26, 0x01, 0},
//
//	{DEV_ADDR_TYPE_DECODER, 0x2C, 0x3A, 0},
//	{DEV_ADDR_TYPE_DECODER, 0x2D, 0x5A, 0},
//	{DEV_ADDR_TYPE_DECODER, 0x2E, 0x40, 0},
//
//	{DEV_ADDR_TYPE_DECODER, 0x30, 0x9E, 0},
//	{DEV_ADDR_TYPE_DECODER, 0x31, 0x20, 0},
//	{DEV_ADDR_TYPE_DECODER, 0x32, 0x10, 0},
//	{DEV_ADDR_TYPE_DECODER, 0x33, 0x90, 0},
	//blue screen
	//{DEV_ADDR_TYPE_DECODER, 0x2A, 0x34, 0},
	/* STD_HDA end */
	/* FHD25 end */
	/* TP2855_decoder_init end */
	/* TP2855_common_init end */

	/* TP2855_mipi_out begin */
	{DEV_ADDR_TYPE_DECODER, TP2855_REG_PAGE, TP2855_PAGE_MIPI, 0},
	{DEV_ADDR_TYPE_DECODER, 0x01, 0xF0, 0},
	{DEV_ADDR_TYPE_DECODER, 0x02, 0x01, 0},
	{DEV_ADDR_TYPE_DECODER, 0x08, 0x0F, 0},
	/* MIPI_4CH4LANE_594M begin */
	{DEV_ADDR_TYPE_DECODER, 0x20, 0x44, 0},
	{DEV_ADDR_TYPE_DECODER, 0x34, 0xE4, 0},
	{DEV_ADDR_TYPE_DECODER, 0x15, 0x0C, 0},
	{DEV_ADDR_TYPE_DECODER, 0x25, 0x08, 0},
	{DEV_ADDR_TYPE_DECODER, 0x26, 0x06, 0},
	{DEV_ADDR_TYPE_DECODER, 0x27, 0x11, 0},
	{DEV_ADDR_TYPE_DECODER, 0x29, 0x0A, 0},
	{DEV_ADDR_TYPE_DECODER, 0x33, 0x07, 0},
	{DEV_ADDR_TYPE_DECODER, 0x33, 0x00, 0},
	{DEV_ADDR_TYPE_DECODER, 0x14, 0x33, 0},
	{DEV_ADDR_TYPE_DECODER, 0x14, 0xB3, 0},
	{DEV_ADDR_TYPE_DECODER, 0x14, 0x33, 0},
	/* MIPI_4CH4LANE_594M end */
	/* TP2855_mipi_out end */
};
#endif
#if 1
static init_data_t decoder_camra_init_data_720p[] = {
	/* TP2855_common_init begin */
	{DEV_ADDR_TYPE_DECODER, TP2855_REG_PAGE, TP2855_PAGE_MIPI, 0},
	{DEV_ADDR_TYPE_DECODER, 0x23, 0x02, 0},
	/* TP2855_decoder_init begin */
	{DEV_ADDR_TYPE_DECODER, TP2855_REG_PAGE, TP2855_PAGE_CH_ALL, 0},
	{DEV_ADDR_TYPE_DECODER, 0x45, 0x01, 0},
	{DEV_ADDR_TYPE_DECODER, 0x06, 0x12, 0},
	{DEV_ADDR_TYPE_DECODER, 0x26, 0x04, 0},
	/* FHD25 begin */
	{DEV_ADDR_TYPE_CAMERA_CUSTOMER_1, 0xF5, 0x00, 0},

	{DEV_ADDR_TYPE_DECODER, 0x02, 0x42, 0},
	{DEV_ADDR_TYPE_DECODER, 0x07, 0xC0, 0},
	{DEV_ADDR_TYPE_DECODER, 0x0B, 0xC0, 0},
	{DEV_ADDR_TYPE_DECODER, 0x0C, 0x13, 0},
	{DEV_ADDR_TYPE_DECODER, 0x0D, 0x50, 0},

	{DEV_ADDR_TYPE_DECODER, 0x15, 0x13, 0},
	{DEV_ADDR_TYPE_DECODER, 0x16, 0x15, 0},
	{DEV_ADDR_TYPE_DECODER, 0x17, 0x00, 0},
	{DEV_ADDR_TYPE_DECODER, 0x18, 0x19, 0},
	{DEV_ADDR_TYPE_DECODER, 0x19, 0xD0, 0},
	{DEV_ADDR_TYPE_DECODER, 0x1A, 0x25, 0},
	//1080p@25fps
//	{DEV_ADDR_TYPE_DECODER, 0x1C, 0x0A, 0},
//	{DEV_ADDR_TYPE_DECODER, 0x1D, 0x50, 0},
	//
	//720p@30fps
	{DEV_ADDR_TYPE_DECODER, 0x1C, 0x06, 0},
	{DEV_ADDR_TYPE_DECODER, 0x1D, 0x72, 0},
	//
	{DEV_ADDR_TYPE_DECODER, 0x20, 0x30, 0},
	{DEV_ADDR_TYPE_DECODER, 0x21, 0x84, 0},
	{DEV_ADDR_TYPE_DECODER, 0x22, 0x36, 0},
	{DEV_ADDR_TYPE_DECODER, 0x23, 0x3C, 0},

	{DEV_ADDR_TYPE_DECODER, 0x2B, 0x60, 0},
	{DEV_ADDR_TYPE_DECODER, 0x2C, 0x0A, 0},
	{DEV_ADDR_TYPE_DECODER, 0x2D, 0x30, 0},
	{DEV_ADDR_TYPE_DECODER, 0x2E, 0x70, 0},

	{DEV_ADDR_TYPE_DECODER, 0x30, 0x48, 0},
	{DEV_ADDR_TYPE_DECODER, 0x31, 0xBB, 0},
	{DEV_ADDR_TYPE_DECODER, 0x32, 0x2E, 0},
	{DEV_ADDR_TYPE_DECODER, 0x33, 0x90, 0},

	{DEV_ADDR_TYPE_DECODER, 0x35, 0x25, 0},
	{DEV_ADDR_TYPE_DECODER, 0x38, 0x00, 0},
	{DEV_ADDR_TYPE_DECODER, 0x39, 0x18, 0},
	/* STD_HDA begin */
	//1080p
//	{DEV_ADDR_TYPE_DECODER, 0x02, 0x44, 0},
//	{DEV_ADDR_TYPE_DECODER, 0x0D, 0x73, 0},
	//
	//720p
	{DEV_ADDR_TYPE_DECODER, 0x02, 0x46, 0},
	{DEV_ADDR_TYPE_DECODER, 0x0D, 0x70, 0},
	//
	{DEV_ADDR_TYPE_DECODER, 0x18, 0x1B, 0},
	{DEV_ADDR_TYPE_DECODER, 0x20, 0x40, 0},
	{DEV_ADDR_TYPE_DECODER, 0x21, 0x46, 0},

	{DEV_ADDR_TYPE_DECODER, 0x25, 0xFE, 0},
	{DEV_ADDR_TYPE_DECODER, 0x26, 0x01, 0},

	{DEV_ADDR_TYPE_DECODER, 0x2C, 0x3A, 0},
	{DEV_ADDR_TYPE_DECODER, 0x2D, 0x5A, 0},
	{DEV_ADDR_TYPE_DECODER, 0x2E, 0x40, 0},

	{DEV_ADDR_TYPE_DECODER, 0x30, 0x9D, 0},
	{DEV_ADDR_TYPE_DECODER, 0x31, 0xCA, 0},
	{DEV_ADDR_TYPE_DECODER, 0x32, 0x01, 0},
	{DEV_ADDR_TYPE_DECODER, 0x33, 0xD0, 0},
	{DEV_ADDR_TYPE_DECODER, 0xF5, 0xFF, 0},//1
	//blue screen
	//{DEV_ADDR_TYPE_DECODER, 0x2A, 0x34, 0},
	/* STD_HDA end */
	/* FHD25 end */
	/* TP2855_decoder_init end */
	/* TP2855_common_init end */

	/* TP2855_mipi_out begin */
	{DEV_ADDR_TYPE_DECODER, TP2855_REG_PAGE, TP2855_PAGE_MIPI, 0},
	{DEV_ADDR_TYPE_DECODER, 0x01, 0xF0, 0},
	{DEV_ADDR_TYPE_DECODER, 0x02, 0x01, 0},
	{DEV_ADDR_TYPE_DECODER, 0x08, 0x0F, 0},
	/* MIPI_4CH4LANE_594M begin */
	{DEV_ADDR_TYPE_DECODER, 0x20, 0x44, 0},
	{DEV_ADDR_TYPE_DECODER, 0x20, 0x44, 0},

	{DEV_ADDR_TYPE_DECODER, 0x34, 0xE4, 0},
	{DEV_ADDR_TYPE_DECODER, 0x14, 0x44, 0},
	{DEV_ADDR_TYPE_DECODER, 0x15, 0x0D, 0},
	{DEV_ADDR_TYPE_DECODER, 0x25, 0x04, 0},
	{DEV_ADDR_TYPE_DECODER, 0x26, 0x03, 0},
	{DEV_ADDR_TYPE_DECODER, 0x27, 0x09, 0},
	{DEV_ADDR_TYPE_DECODER, 0x29, 0x02, 0},
	{DEV_ADDR_TYPE_DECODER, 0x33, 0x07, 0},
	{DEV_ADDR_TYPE_DECODER, 0x33, 0x00, 0},
	{DEV_ADDR_TYPE_DECODER, 0x14, 0xC4, 0},
	{DEV_ADDR_TYPE_DECODER, 0x14, 0x44, 0},
	/* MIPI_4CH4LANE_594M end */
	/* TP2855_mipi_out end */
};
#endif

static init_data_t decoder_camra_disable_data[] = {
	{DEV_ADDR_TYPE_DECODER, TP2855_REG_PAGE, TP2855_PAGE_MIPI, 0},
	{DEV_ADDR_TYPE_DECODER, 0x22, 0x00, 0},
	{DEV_ADDR_TYPE_DECODER, 0x23, 0x02, 0},
};

static init_data_t decoder_camra_enable_data[] = {
	{DEV_ADDR_TYPE_DECODER, TP2855_REG_PAGE, TP2855_PAGE_MIPI, 0},
	{DEV_ADDR_TYPE_DECODER, 0x23, 0x00, 0},
	{DEV_ADDR_TYPE_DECODER, 0x22, 0x0F, 0},
};

capture_context_t capture_create_context(uint32_t flags)
{
	capture_context_t context;

	capture_log_init();
	log_context = capture_log_create_context(CAPTURE_DEV_TYPE_DECODER_CAMERA, "tp2855_no3");
	if (log_context < 0) {
		goto error_log;
	}
	capture_log_debug(log_context, 2, "(%s) flags[0x%08x]\n", __func__, flags);

	context = calloc(1, sizeof(struct _capture_context));
	if (!context) {
		capture_log_error(log_context, "calloc error\n");
		goto error_calloc;
	}

	memset(context->i2c_path, 0x00, sizeof(context->i2c_path));
	context->camera_use = CAPTURE_CAMERA_USE_0;
	context->i2c_fd = -1;
	context->decoder_addr = 0x48;

	return context;

error_calloc:
error_log:
	return NULL;
}

void capture_destroy_context(capture_context_t context)
{
	capture_log_debug(log_context, 2, "(%s) context[0x%p]\n", __func__, context);
	context->decoder_addr = 0x48;
	if (context->i2c_fd != -1) {
		close(context->i2c_fd);
		context->i2c_fd = -1;
	}
	context->camera_use = CAPTURE_CAMERA_USE_0;
	memset(context->i2c_path, 0x00, sizeof(context->i2c_path));

	capture_log_destroy_context(log_context);
}

static int capture_get_camera_status(capture_context_t context)
{
	uint8_t lock;
	int status = 0;
	int i;

	for (i = 0; i < context->camera_use; i++) {
		if (capture_i2c_wr(context->i2c_fd, context->decoder_addr, TP2855_REG_PAGE, i) != 0) {
			capture_log_error(log_context, "decoder_camera fail to write device page %d(%s)\n", errno, strerror(errno));
			goto error;
		}

		if (capture_i2c_rd(context->i2c_fd, context->decoder_addr, TP2855_REG_VIDEO_INPUT_STATUS, &lock) != 0) {
			capture_log_error(log_context, "decoder_camera fail to read device lock status %d(%s)\n", errno, strerror(errno));
			goto error;
		}

		//capture_log_debug(log_context, 2, "decoder_camera camera %d lock status %#04x\n", i, lock);
		if (((lock & 0x71) == 0x70) || ((lock & 0x71) == 0x61)) {
			status |= (1 << i);
		}
	}

	return status;
error:
	return 0x00;
}

int capture_get_property_i(capture_context_t context, uint32_t prop, int32_t *value)
{
	//capture_log_debug(log_context, 2, "(%s) context[0x%p] prop[%s] value[0x%p]\n", __func__, context, capture_log_prop_str(prop), value);
	switch (prop) {
	case CAPTURE_PROPERTY_CAMERA_STATUS:
		if(value == 1)
			capture_dump_decoder_regs(context);
		else
			return capture_get_camera_status(context);
	default:
		goto error;
	}

	return 0;

error:
	return EINVAL;
}

int capture_get_property_p(capture_context_t context, uint32_t prop, void **value)
{
	capture_log_debug(log_context, 2, "(%s) context[0x%p] prop[%s] value[0x%p]\n", __func__, context, capture_log_prop_str(prop), value);
	switch (prop) {
	default:
		goto error;
	}

	return 0;

error:
	return EINVAL;
}

int capture_is_property(capture_context_t context, uint32_t prop)
{
	capture_log_debug(log_context, 2, "(%s) context[0x%p] prop[%s]\n", __func__, context, capture_log_prop_str(prop));
	if ((prop == CAPTURE_PROPERTY_DECODER_I2C_PATH)
		|| (prop == CAPTURE_PROPERTY_DECODER_I2C_ADDR)
		|| (prop == CAPTURE_PROPERTY_VERBOSITY)
		|| (prop == CAPTURE_PROPERTY_CAMERA_USE)
		|| (prop == CAPTURE_PROPERTY_CAMERA_STATUS)
		|| (prop == CAPTURE_PROPERTY_DECODER_ENABLE)) {
		return 1;
	}

	return 0;
}

static int capture_decoder_disable(capture_context_t context)
{
	int i;
	uint32_t addr;

	for (i = 0; i < sizeof(decoder_camra_disable_data) / sizeof(decoder_camra_disable_data[0]); i++) {
		if (decoder_camra_disable_data[i].addr_type == DEV_ADDR_TYPE_DECODER) {
			addr = context->decoder_addr;
		}
		else {
			capture_log_error(log_context, "decoder_camera unknown addr type %d\n", decoder_camra_disable_data[i].addr_type);
			goto error;
		}

		if (capture_i2c_wr(context->i2c_fd, addr, decoder_camra_disable_data[i].reg, decoder_camra_disable_data[i].data) != 0) {
			capture_log_error(log_context, "decoder_camera disable i2c write reg %#04x value %#04x error\n", decoder_camra_disable_data[i].reg, decoder_camra_disable_data[i].data);
			goto error;
		}
		if (decoder_camra_disable_data[i].delay != 0)
			delay(decoder_camra_disable_data[i].delay);
	}

	return 0;

error:
	return -1;
}

static int capture_decoder_enable(capture_context_t context)
{
	int i;
	uint32_t addr;

	for (i = 0; i < sizeof(decoder_camra_enable_data) / sizeof(decoder_camra_enable_data[0]); i++) {
		if (decoder_camra_enable_data[i].addr_type == DEV_ADDR_TYPE_DECODER) {
			addr = context->decoder_addr;
		}
		else {
			capture_log_error(log_context, "decoder_camera unknown addr type %d\n", decoder_camra_enable_data[i].addr_type);
			goto error;
		}

		if (capture_i2c_wr(context->i2c_fd, addr, decoder_camra_enable_data[i].reg, decoder_camra_enable_data[i].data) != 0) {
			capture_log_error(log_context, "decoder_camera enable i2c write reg %#04x value %#04x error\n", decoder_camra_enable_data[i].reg, decoder_camra_enable_data[i].data);
			goto error;
		}
		if (decoder_camra_enable_data[i].delay != 0)
			delay(decoder_camra_enable_data[i].delay);
	}

	return 0;

error:
	return -1;
}

static int decoder_enable(capture_context_t context, int enable)
{
	int ret;

	if ((ret = tp2855_open(context)) != 0) {
		capture_log_error(log_context, "decoder_camera tp2855 open error\n");
	}
	else {
		if (!!enable == CAPTURE_DECODER_ENABLE) {
			ret = capture_decoder_enable(context);
		}
		else {
			ret = capture_decoder_disable(context);
		}
	}

	return ret;
}

int capture_set_property_i(capture_context_t context, uint32_t prop, int32_t value)
{
	capture_log_debug(log_context, 2, "(%s) context[0x%p] prop[%s] value[%d(0x%08x)]\n", __func__, context, capture_log_prop_str(prop), value, value);

	switch (prop) {
	case CAPTURE_PROPERTY_DECODER_I2C_ADDR:
		context->decoder_addr = value;
		break;
	case CAPTURE_PROPERTY_VERBOSITY:
		capture_log_set_verbose(log_context, value);
		break;
	case CAPTURE_PROPERTY_CAMERA_USE:
		context->camera_use = value;
		break;
	case CAPTURE_PROPERTY_DECODER_ENABLE:
		return decoder_enable(context, value);
	case CAPTURE_PROPERTY_CAMERA_TYPE:
		camera_type = 0;
		break;
	default:
		capture_log_error(log_context, "decoder_camera unknown prop %s\n", capture_log_prop_str(prop));
		goto error;
	}

	return 0;

error:
	return -1;
}

int capture_set_property_p(capture_context_t context, uint32_t prop, void *value)
{
	capture_log_debug(log_context, 2, "(%s) context[0x%p] prop[%s] value[%s]\n", __func__, context, capture_log_prop_str(prop), (char *)value);

	switch (prop) {
	case CAPTURE_PROPERTY_DECODER_I2C_PATH:
		strncpy(context->i2c_path, value, PATH_MAX);
		context->i2c_path[PATH_MAX] = '\0';
		break;
	default:
		capture_log_error(log_context, "decoder_camera unknown prop %s\n", capture_log_prop_str(prop));
		goto error;
	}

	return 0;

error:
	return -1;
}

static int tp2855_open(capture_context_t context)
{
	uint8_t data1;
	uint8_t data2;

	if (context->i2c_fd > 0)
		return 0;

	if(strlen(context->i2c_path) == 0) {
		capture_log_error(log_context, "decoder_camera decoder i2c path is not set\n");
		return -ENODEV;
	}

	if((context->i2c_fd = capture_i2c_open(context->i2c_path)) == -1) {
		capture_log_error(log_context, "decoder_camera decoder i2c device %s open error %d(%s)\n", context->i2c_path, errno, strerror(errno));
		goto error;
	}

	capture_log_debug(log_context, 2, "decoder_camera fd: %d, decoder: %#04x@%s\n", context->i2c_fd, context->decoder_addr, context->i2c_path);

	if(capture_i2c_set_speed(context->i2c_fd, 400000) != EOK) {
		capture_log_error(log_context, "decoder_camera set i2c speed error %d(%s)\n", errno, strerror(errno));
		goto error_set;
	}

	// check device identifiler
	if (capture_i2c_wr(context->i2c_fd, context->decoder_addr, TP2855_REG_PAGE, 0) != 0) {
		capture_log_error(log_context, "decoder_camera fail to write device page %d(%s)\n", errno, strerror(errno));
		goto error;
	}
	if (capture_i2c_rd(context->i2c_fd, context->decoder_addr, TP2855_REG_DEVICE_ID1, &data1) != 0) {
		capture_log_error(log_context, "decoder_camera fail to read device identifer1 %d(%s)\n", errno, strerror(errno));
		goto error_read;
	}
	if (capture_i2c_rd(context->i2c_fd, context->decoder_addr, TP2855_REG_DEVICE_ID2, &data2) != 0) {
		capture_log_error(log_context, "decoder_camera fail to read device identifer2 %d(%s)\n", errno, strerror(errno));
		goto error_read;
	}
	if((data1 != TP2855_DEVICE_ID1) || (data2 != TP2855_DEVICE_ID2)) {
		capture_log_error(log_context, "decoder_camera unknown device1 %#04x/%#04x or device2 %#04x/%#04x\n", data1, TP2855_DEVICE_ID1, data2, TP2855_DEVICE_ID2);
		goto error_check;
	}

	return 0;

error_check:
error_read:
error_set:
	capture_i2c_close(context->i2c_fd);
	context->i2c_fd = -1;

error:
	return -1;
}

static void capture_dump_regs(int fd, uint32_t addr)
{
#define REG_DUMP_CNT 256
	int i = 0, j = 0;
	uint8_t data[REG_DUMP_CNT];
	char log[2048] = {0};
	capture_i2c_dump(fd, addr, 0x00, data, REG_DUMP_CNT);
	sprintf(&log[strlen(log)], "\n    0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f\n");
	for (i = 0; i < REG_DUMP_CNT; i++) {
		if ((i & 0xf) == 0) {
			sprintf(&log[strlen(log)], "%02x:", j);
		}
		sprintf(&log[strlen(log)], " %02x", data[i]);
		if ((i & 0xf) == 0x0f) {
			sprintf(&log[strlen(log)], "\n");
		}
		j++;
	}
	sprintf(&log[strlen(log)], "\n");

	capture_log_debug(log_context, 2, log);
}

static void capture_dump_decoder_regs(capture_context_t context)
{
	if (capture_i2c_wr(context->i2c_fd, context->decoder_addr, TP2855_REG_PAGE, TP2855_PAGE_CH0) != 0) {
		capture_log_error(log_context, "decoder_camera fail to write device page %d(%s)\n", errno, strerror(errno));
		goto error;
	}
	capture_dump_regs(context->i2c_fd, context->decoder_addr);

	if (capture_i2c_wr(context->i2c_fd, context->decoder_addr, TP2855_REG_PAGE, TP2855_PAGE_CH1) != 0) {
		capture_log_error(log_context, "decoder_camera fail to write device page %d(%s)\n", errno, strerror(errno));
		goto error;
	}
	capture_dump_regs(context->i2c_fd, context->decoder_addr);

	if (capture_i2c_wr(context->i2c_fd, context->decoder_addr, TP2855_REG_PAGE, TP2855_PAGE_CH2) != 0) {
		capture_log_error(log_context, "decoder_camera fail to write device page %d(%s)\n", errno, strerror(errno));
		goto error;
	}
	capture_dump_regs(context->i2c_fd, context->decoder_addr);

	if (capture_i2c_wr(context->i2c_fd, context->decoder_addr, TP2855_REG_PAGE, TP2855_PAGE_CH3) != 0) {
		capture_log_error(log_context, "decoder_camera fail to write device page %d(%s)\n", errno, strerror(errno));
		goto error;
	}
	capture_dump_regs(context->i2c_fd, context->decoder_addr);

	if (capture_i2c_wr(context->i2c_fd, context->decoder_addr, TP2855_REG_PAGE, TP2855_PAGE_MIPI) != 0) {
		capture_log_error(log_context, "decoder_camera fail to write device page %d(%s)\n", errno, strerror(errno));
		goto error;
	}
	capture_dump_regs(context->i2c_fd, context->decoder_addr);

error:
	return;
}

static int capture_decoder_camera_init(capture_context_t context)
{
	int i;
	uint8_t data;

	for (i = 0; i < sizeof(decoder_camra_init_data) / sizeof(decoder_camra_init_data[0]); i++) {
		if (decoder_camra_init_data[i].addr_type == DEV_ADDR_TYPE_DECODER) {
		}
		else if (decoder_camra_init_data[i].addr_type == DEV_ADDR_TYPE_CAMERA_CUSTOMER_1) {
			if (capture_i2c_rd(context->i2c_fd, context->decoder_addr, decoder_camra_init_data[i].reg, &data) != 0) {
				capture_log_error(log_context, "decoder_camera init i2c read reg %#04x error\n", decoder_camra_init_data[i].reg);
				goto error;
			}
			decoder_camra_init_data[i].data = data & ~0x0F;
		}
		else {
			capture_log_error(log_context, "decoder_camera unknown addr type %d\n", decoder_camra_init_data[i].addr_type);
			goto error;
		}

		if (capture_i2c_wr(context->i2c_fd, context->decoder_addr, decoder_camra_init_data[i].reg, decoder_camra_init_data[i].data) != 0) {
			capture_log_error(log_context, "decoder_camera init i2c write reg %#04x value %#04x error\n", decoder_camra_init_data[i].reg, decoder_camra_init_data[i].data);
			goto error;
		}
		if (decoder_camra_init_data[i].delay != 0)
			delay(decoder_camra_init_data[i].delay);
	}

	return 0;

error:
	return -1;
}

static int capture_decoder_camera_init_720p(capture_context_t context)
{
	int i;
	uint8_t data;

	for (i = 0; i < sizeof(decoder_camra_init_data_720p) / sizeof(decoder_camra_init_data_720p[0]); i++) {
		if (decoder_camra_init_data_720p[i].addr_type == DEV_ADDR_TYPE_DECODER) {
		}
		else if (decoder_camra_init_data_720p[i].addr_type == DEV_ADDR_TYPE_CAMERA_CUSTOMER_1) {
			if (capture_i2c_rd(context->i2c_fd, context->decoder_addr, decoder_camra_init_data_720p[i].reg, &data) != 0) {
				capture_log_error(log_context, "decoder_camera init i2c read reg %#04x error\n", decoder_camra_init_data_720p[i].reg);
				goto error;
			}
			decoder_camra_init_data_720p[i].data = data & ~0x0F;
		}
		else {
			capture_log_error(log_context, "decoder_camera unknown addr type %d\n", decoder_camra_init_data_720p[i].addr_type);
			goto error;
		}

		if (capture_i2c_wr(context->i2c_fd, context->decoder_addr, decoder_camra_init_data_720p[i].reg, decoder_camra_init_data_720p[i].data) != 0) {
			capture_log_error(log_context, "decoder_camera init i2c write reg %#04x value %#04x error\n", decoder_camra_init_data_720p[i].reg, decoder_camra_init_data_720p[i].data);
			goto error;
		}
		if (decoder_camra_init_data_720p[i].delay != 0)
			delay(decoder_camra_init_data_720p[i].delay);
	}

	return 0;

error:
	return -1;
}

static int capture_decoder_camera_status_check(capture_context_t context)
{
#define CHECK_RETRY_TIMES 50
	uint32_t retry;
	int status;
	int check;

	switch (context->camera_use) {
	case CAPTURE_CAMERA_USE_1:
		check = 0x01;
		break;
	case CAPTURE_CAMERA_USE_2:
		check = 0x03;
		break;
	case CAPTURE_CAMERA_USE_3:
		check = 0x07;
		break;
	case CAPTURE_CAMERA_USE_4:
		check = 0x0F;
		break;
	default:
		capture_log_error(log_context, "decoder_camera unknown camera use %d\n", context->camera_use);
		goto error;
	}

	for (retry = 0; retry < CHECK_RETRY_TIMES; retry++) {
		status = capture_get_camera_status(context);
		if (status == check) {
			break;
		}

		delay(100);
	}

	if (retry == CHECK_RETRY_TIMES) {
		capture_log_error(log_context, "decoder_camera status check error %#04x\n", status);
		goto error;
	}
	capture_log_debug(log_context, 2, "decoder_camera status check ok %#04x\n", status);

	return 0;

error:
	return -1;
}

int capture_update(capture_context_t context, uint32_t flags)
{
	int error;

	capture_log_debug(log_context, 2, "(%s) context[0x%p] flags[0x%08x]\n", __func__, context, flags);
    /*todo: multiple opening i2c device here*/
	if ((error = tp2855_open(context)) != 0) {
		capture_log_error(log_context, "decoder_camera tp2855 open error\n");
		return error;
	}

	printf("\ncamera_type is %d \n\n", camera_type);

	if(camera_type){
		//capture_decoder_camera_init_720p
		if ((error = capture_decoder_camera_init_720p(context)) != 0) {
			capture_log_error(log_context, "decoder_camera init error\n");
			return error;
		}
	}else{
		if ((error = capture_decoder_camera_init(context)) != 0) {
			capture_log_error(log_context, "decoder_camera init error\n");
			return error;
		}
	}

    /*Capture application will periodly check the camera status, so we do not need to check here*/
//	if ((error = capture_decoder_camera_status_check(context)) != 0) {
//		capture_log_error(log_context, "decoder_camera check status error\n");
//		//return error;
//	}

	capture_dump_decoder_regs(context);

	return 0;
}

int capture_get_reg(capture_context_t context, uint32_t page, uint32_t reg, uint32_t* data)
{
	uint8_t u8_data[1];
	int ret = -1;
	capture_log_error(log_context, "[capture_get_reg]::context->i2c_fd = %d\n", context->i2c_fd);
	if (context->i2c_fd < 0){
		if ((ret = tp2855_open(context)) != 0) {
			capture_log_error(log_context, "decoder_camera tp2855 open error\n");
		}
	}
	ret = capture_i2c_wr(context->i2c_fd, context->decoder_addr, TP2855_REG_PAGE, page);
	if (ret != 0) {
		capture_log_error(log_context, "decoder_camera fail to write device page %d(%s)\n", errno, strerror(errno));
		goto error;
	}

	capture_i2c_rd(context->i2c_fd, context->decoder_addr, reg, u8_data);
	*data = u8_data[0];

	return 0;
error:
	capture_log_error(log_context, "context->i2c_fd[%d]page[%d]reg[0x%02X]ret[%d].\r\n\n", context->i2c_fd, page, reg, ret);
	return (-1);
}

//=======================================================================================================================================
#include "tp2855_reg.h"

extern void TP2855_mipi_out(uint8_t output);
extern void TP2855_decoder_init(uint8_t ch, uint8_t fmt, uint8_t std);
extern void TP2855_common_init(uint8_t fmt, uint8_t std, uint8_t mipi_config);

static camera_decoder_t * camera_decoder = NULL;

static int write_reg(uint8_t reg, uint8_t value){
	int ret = EXIT_FAILURE;
	uint8_t check_value = 0;
	if(camera_decoder->i2c_fd == -1){
		printf("[%s]:i2c_fd = %d\n", __func__, camera_decoder->i2c_fd);
		return EXIT_FAILURE;
	}

	ret = capture_i2c_wr(camera_decoder->i2c_fd, camera_decoder->decoder_addr, reg, value);
	if (ret != EOK) {
		//capture_log_error(log_context, "decoder_camera fail to write device page %d(%s)\n", errno, strerror(errno));
		printf("Decoder_camera fail to write reg[%d] and value[%d]\n", reg, value);
		return ret;
	}

	if(camera_decoder->read_back_flag){
		ret = capture_i2c_rd(camera_decoder->i2c_fd, camera_decoder->decoder_addr, reg, &check_value);
		if(ret != EOK){
			printf("Decoder_camera fail to read back\n");
			return ret;
		}
		if(check_value != value){
			printf("Decoder_camera fail to write reg[%d] and value[%d][Read back]\n", reg, value);
			return EXIT_FAILURE;
		}
	}

	return EXIT_SUCCESS;
}

static int tp2855_dump_regs(int fd, uint8_t addr){
#define REG_DUMP_CNT 256
	int i = 0, j = 0;
	uint8_t data[REG_DUMP_CNT];
	char log[2048] = {0};
	capture_i2c_dump(fd, addr, 0x00, data, REG_DUMP_CNT);
	sprintf(&log[strlen(log)], "\n    0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f\n");
	for (i = 0; i < REG_DUMP_CNT; i++) {
		if ((i & 0xf) == 0) {
			sprintf(&log[strlen(log)], "%02x:", j);
		}
		sprintf(&log[strlen(log)], " %02x", data[i]);
		if ((i & 0xf) == 0x0f) {
			sprintf(&log[strlen(log)], "\n");
		}
		j++;
	}
	sprintf(&log[strlen(log)], "\n");

	printf("%s", log);
	return EXIT_SUCCESS;
}

static int decoder_dump_regs(){
	uint8_t page_map[] = {0, 1, 2, 3, 8};
	int i = 0, ret = EXIT_FAILURE;
	for(i = 0; i < sizeof(page_map)/sizeof(uint8_t); i++){
		ret = capture_i2c_wr(camera_decoder->i2c_fd, camera_decoder->decoder_addr, TP2855_REG_PAGE, page_map[i]);
		if (ret != EOK) {
			printf("Decoder_camera fail to write device page %d\n", page_map[i]);
			return ret;
		}

		tp2855_dump_regs(camera_decoder->i2c_fd, camera_decoder->decoder_addr);
	}

	return EXIT_SUCCESS;
}

static int decoder_mipi_out(uint8_t enable){
	int ret = EXIT_FAILURE;

	ret = write_reg(TP2855_REG_PAGE, MIPI_PAGE);
	if(ret != 0){
		printf("%d\n", __LINE__);
		return EXIT_FAILURE;
	}
	if(enable){
		ret = write_reg(0x23, 0x00);
		if(ret != 0){
			printf("%d\n", __LINE__);
			return EXIT_FAILURE;
		}
		ret = write_reg(0x22, 0x0F);
		if(ret != 0){
			printf("%d\n", __LINE__);
			return EXIT_FAILURE;
		}
	}else{
		ret = write_reg(0x22, 0x00);
		if(ret != 0){
			printf("%d\n", __LINE__);
			return EXIT_FAILURE;
		}
		ret = write_reg(0x23, 0x02);
		if(ret != 0){
			printf("%d\n", __LINE__);
			return EXIT_FAILURE;
		}
	}

	return EXIT_SUCCESS;
}
static int decoder_init_reg_map(uint8_t channel){
	//int ret = EXIT_FAILURE;
	int i = 0;
	if(channel == CH_ALL){
		TP2855_common_init(camera_decoder->fmt[CH_1], camera_decoder->std[CH_1], camera_decoder->mipi_config);
	}else{
		for(i = 0 ; i < CH_ALL; i++){
			TP2855_decoder_init(i, camera_decoder->fmt[i], camera_decoder->std[i]);
		}
		TP2855_mipi_out(camera_decoder->mipi_config);
	}

	return EXIT_SUCCESS;
}


/*#######################	Use for tp2855_reg.c	#######################*/
int tp28xx_write_reg(uint8_t reg, uint8_t value){
	int ret = EXIT_FAILURE;
	ret  = capture_i2c_wr(camera_decoder->i2c_fd, camera_decoder->decoder_addr, reg, value);
	if(ret != EOK){

	}
	return ret;
}

uint8_t tp28xx_read_reg(uint8_t reg){
	int ret = EXIT_FAILURE;
	uint8_t value;
	ret =  capture_i2c_rd(camera_decoder->i2c_fd, camera_decoder->decoder_addr, reg, &value);
	if(ret != EOK){

	}
	return value;
}
/***************************************************************************************************/

/*#######################	export to user	#######################*/
int camera_decoder_init(camera_decoder_t ** decoder){
	int ret = EXIT_FAILURE;
	*decoder = (camera_decoder_t *)calloc(1, sizeof(camera_decoder_t));
	camera_decoder = *decoder;
	if(decoder == NULL){
		printf("Try to calloc camera_decoder_t memory fail\n");
		return EXIT_FAILURE;
	}

	ret = pthread_mutex_init(&(*decoder)->mutex, NULL);
	if(ret != EOK){
		printf("Try to init mutex fail\n");
		return ret;
	}

	(*decoder)->i2c_fd = -1;
	return EXIT_SUCCESS;
}

int camera_decoder_deinit(camera_decoder_t * decoder){
	pthread_mutex_destroy(&decoder->mutex);
	close(decoder->i2c_fd);

	free(camera_decoder);
	return EXIT_SUCCESS;
}

int camera_decoder_set_property(camera_decoder_t * decoder, int propetry, void * data){
	int ret = EXIT_FAILURE;

	switch(propetry){
		case DECODER_I2C_PATH:
			strcpy(decoder->i2c_path, (char *)data);
			decoder->i2c_fd = capture_i2c_open(decoder->i2c_path);
			if(decoder->i2c_fd == -1){
				printf("Can not open i2c_path: %s\n", (char *)data);
				return EXIT_FAILURE;
			}
			break;
		case DECODER_FMT:
			if(*(uint8_t *)data > 3){
				decoder->fmt[0] = *((uint8_t *)data + 1);
				decoder->fmt[1] = *((uint8_t *)data + 1);
				decoder->fmt[2] = *((uint8_t *)data + 1);
				decoder->fmt[3] = *((uint8_t *)data + 1);
			}else{
				decoder->fmt[*(uint8_t *)data] = *((uint8_t *)data + 1);
			}
			break;
		case DECODER_STD:
			if(*(uint8_t *)data > 3){
				decoder->std[0] = *((uint8_t *)data + 1);
				decoder->std[1] = *((uint8_t *)data + 1);
				decoder->std[2] = *((uint8_t *)data + 1);
				decoder->std[3] = *((uint8_t *)data + 1);
			}else{
				decoder->std[*(uint8_t *)data] = *((uint8_t *)data + 1);
			}
			break;
		case DECODER_READ_BACK_FLAG:
			decoder->read_back_flag = *(uint8_t *)data;
			break;
		case DECODER_INIT_REG_MAP:
			ret = decoder_init_reg_map(*(uint8_t *)data);
			return ret;
			break;
		case DECODER_MIPI_OUT:
			ret = decoder_mipi_out(*(uint8_t *)data);
			return ret;
			break;
		case DECODER_DUMP_REGISTER:
			decoder_dump_regs();
			break;
		default:
			break;
	}
	return EXIT_SUCCESS;
}

int camera_decoder_read_reg(uint8_t page, uint8_t reg, uint8_t * value){
	int ret = EXIT_FAILURE;
	if(camera_decoder->i2c_fd == -1){
		printf("[%s]:i2c_fd = %d\n",__func__, camera_decoder->i2c_fd);
		return EXIT_FAILURE;
	}
	if(camera_decoder->mutex_flag)
		pthread_mutex_lock(&camera_decoder->mutex);

	ret = capture_i2c_wr(camera_decoder->i2c_fd, camera_decoder->decoder_addr, TP2855_REG_PAGE, page);
	if (ret != EOK) {
		//capture_log_error(log_context, "decoder_camera fail to write device page %d(%s)\n", errno, strerror(errno));
		printf("Decoder_camera fail to write device page %d\n", page);
		return ret;
	}

	if(camera_decoder->read_back_flag){
		ret = capture_i2c_rd(camera_decoder->i2c_fd, camera_decoder->decoder_addr, TP2855_REG_PAGE, value);
		if(ret != EOK){
			printf("Decoder_camera fail to read back\n");
			return ret;
		}
		if(*value != page){
			printf("Decoder_camera fail to write device page %d[Read back]\n", page);
			return EXIT_FAILURE;
		}
	}

	ret = capture_i2c_rd(camera_decoder->i2c_fd, camera_decoder->decoder_addr, reg, value);
	if(ret != EOK){
		printf("Decoder_camera fail to write device page %d[Read back]\n", page);
		return ret;
	}

	if(camera_decoder->mutex_flag)
		pthread_mutex_unlock(&camera_decoder->mutex);
	return EXIT_SUCCESS;
}

int camera_decoder_write_reg(uint8_t page, uint8_t reg, uint8_t value){
	int ret = EXIT_FAILURE;
	uint8_t check_value = 0;
	if(camera_decoder->i2c_fd == -1){
		printf("[%s]:i2c_fd = %d\n", __func__, camera_decoder->i2c_fd);
		return EXIT_FAILURE;
	}
	if(camera_decoder->mutex_flag)
		pthread_mutex_lock(&camera_decoder->mutex);

	ret = capture_i2c_wr(camera_decoder->i2c_fd, camera_decoder->decoder_addr, TP2855_REG_PAGE, page);
	if (ret != EOK) {
		//capture_log_error(log_context, "decoder_camera fail to write device page %d(%s)\n", errno, strerror(errno));
		printf("Decoder_camera fail to write device page %d\n", page);
		return ret;
	}

	if(camera_decoder->read_back_flag){
		ret = capture_i2c_rd(camera_decoder->i2c_fd, camera_decoder->decoder_addr, TP2855_REG_PAGE, &check_value);
		if(ret != EOK){
			printf("Decoder_camera fail to read back\n");
			return ret;
		}
		if(check_value != page){
			printf("Decoder_camera fail to write device page %d[Read back]\n", page);
			return EXIT_FAILURE;
		}
	}

	ret = capture_i2c_wr(camera_decoder->i2c_fd, camera_decoder->decoder_addr, reg, value);
	if (ret != EOK) {
		//capture_log_error(log_context, "decoder_camera fail to write device page %d(%s)\n", errno, strerror(errno));
		printf("Decoder_camera fail to write reg[%d] and value[%d]\n", reg, value);
		return ret;
	}

	if(camera_decoder->read_back_flag){
		ret = capture_i2c_rd(camera_decoder->i2c_fd, camera_decoder->decoder_addr, reg, &check_value);
		if(ret != EOK){
			printf("Decoder_camera fail to read back\n");
			return ret;
		}
		if(check_value != value){
			printf("Decoder_camera fail to write reg[%d] and value[%d][Read back]\n", reg, value);
			return EXIT_FAILURE;
		}
	}

	if(camera_decoder->mutex_flag)
		pthread_mutex_unlock(&camera_decoder->mutex);
	return EXIT_SUCCESS;
}
/***************************************************************************************************/
